#include <joyos.h>

#include "define.h"
#include "led.h"
#include "roboturn.h"
#include "init.h"

/**
 * initPose check initial position and orientation
 * 
 * @requires none
 * @effect set initial position and orientation
 * @modifies initPlace, initOrient, currentSide, currentOrient
 */
void initPose(void){
	uint8_t led[3];
	uint8_t led_read[3];
	uint8_t check;
	
	led[0] = LED_UNDER_FRONTRIGHT;
	led[1] = LED_UNDER_FRONTLEFT;
	led[2] = LED_UNDER_BACKRIGHT; 
	
	for (uint8_t j=0;j<3;j++) {
		led_read[j] = isBlack(led[j]);
	}
	
	check = led_read[0] + led_read[1] + led_read[2];
	if (check >= 2) {
		initPlace = 1;
		
		if (led_read[0] == 0) {initOrient = 0;}
		else if (led_read[1] == 0) {initOrient = 1;}
		else if (led_read[2] == 0) {initOrient = 3;}
		else {initOrient = 2;}
	}
	else if (check <= 1) {
		initPlace = 0;
		
		if (led_read[0] == 1) {initOrient = 1;}
		else if (led_read[1] == 1) {initOrient = 2;}
		else if (led_read[2] == 1) {initOrient = 0;}
		else {initOrient = 3;}
	}
	else {
		printf("\nSuay Laew la..");
	}
	
	currentOrient = initOrient;
	if ((initOrient == 0) || (initOrient == 2))
		{currentSide = 3;}
	else if ((initOrient == 1) || (initOrient == 3)){
		if (initPlace == 0)
			currentSide = 0;
		else
			currentSide = 2;
	}
	else{
		printf("\nOrientation Error: orient = %d", initOrient);
	}
	
	//test only
	//printf ("\nFR: %d FL: %d BR: %d PL: %d OR: %d", led_read[0], led_read[1], led_read[2], initPlace, initOrient);
}

/*
 * initTurn turns the robot towards its proper initial position
 *
 * The current proper orientation is 1
 *
 * @requires 0 <= currentOrient <= 3
 * @effects robot is turned towards orientation 1 
 * @modifies currentOrient, currentSide
 */
int initTurn(void){
	turnToOrient(1);
	gyro_set_degrees(0); //set 1 to 0 degree of gyro
	return 0;
}

